#!/usr/bin/env python3
"""
source install/setup.zsh
测试可视化 rviz2 -d /ws_618/src/manipulation/scripts/demo/debug.rviz
直接运行本代码启动D435相机及识别 python3 src/manipulation/scripts/mani_core.py

离线调试方法:
使用ros2 bag record /camera/color/image_raw /camera/depth/image_raw 记录rosbag数据
使用DummyCamera加载离线rosbag数据 python3 src/manipulation/scripts/mani_core.py --ros-args -p camera_class:=DummyCamera

调试YOLO识别:
python3 src/manipulation/scripts/mani_core.py --ros-args -p camera_class:=DummyCamera -p detector:=yolo

测试YOLO识别:
python3 src/manipulation/scripts/mani_core.py --ros-args -p detector:=yolo

测试Apriltag识别:
python3 src/manipulation/scripts/mani_core.py --ros-args -p detector:=apriltag
"""

import rclpy
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
from manipulation.scripts.detection import ApriltagDetectionNode, YoloDetectionNode
from manipulation.scripts.mani_connector import ManiConnectorNode

class ManiCoreNode(Node):
    def __init__(self):
        super().__init__('mani_core_node')
        self.declare_parameter('detector', 'yolo')  # Options: ['apriltag', 'yolo']
        detector = self.get_parameter('detector').get_parameter_value().string_value

        if detector == 'apriltag':
            self.detection_node = ApriltagDetectionNode()
        elif detector == 'yolo':
            self.detection_node = YoloDetectionNode()
        else:
            self.get_logger().error(f"Detector '{detector}' is not supported. Options: ['apriltag', 'yolo'].")
            rclpy.shutdown()
        self.mani_connector = ManiConnectorNode(self.detection_node)
        self.get_logger().info("ManiCoreNode initialized")

    def run(self):
        self.get_logger().info("Running ManiCore Node")
        # Here you can add the main logic of your manipulation core

if __name__ == '__main__':
    try:
        rclpy.init()
        node = ManiCoreNode()
        executor = MultiThreadedExecutor()
        executor.add_node(node)
        executor.add_node(node.detection_node)
        executor.add_node(node.mani_connector)
        executor.spin()
    except Exception as e:
        print(f"An error occurred: {e}")
        executor.shutdown()
        rclpy.shutdown()
